COMMENT ⊗   VALID 00021 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00003 00002		TITLE SERVO
C00006 00003	Shared ram. Check all refs.
C00008 00004	Add SEI and CLI to all shared ram refs.
C00010 00005	Position command?
C00012 00006	Clock tick interrupt.
C00013 00007		JSR	POSUPD	Put POSUPD here?
C00015 00008		BITZ	MODE	?If servo is disabled, we're
C00017 00009	INBAND:	LDAI	LSBENB	In band. Is LSB servo enabled
C00018 00010		LDAZ	LOGTMP	Since the arithmetic routines
C00020 00011	Change CMDEND for no host command interrupt?
C00022 00012		JSR	LOG	...float...
C00024 00013	INTBL:		IMMEDIATE COMMAND TABLE?
C00025 00014	Subroutines?
C00028 00015	DAC output subroutine. Not sub?
C00030 00016	No index?
C00031 00017	Arithmetic routines.
C00034 00018	BLOATE:	JSR	UNNORM	Yes. Unnormalize, then
C00037 00019		BPL	INSIGN	Result in range?
C00039 00020	DEFERRED COMMANDS.
C00040 00021	Immediate commands.
C00043 ENDMK
C⊗;
	TITLE SERVO
	.INSERT ASMBL.FAI[CMS,LCS]

;I/O address definitions.
   DAC ← 100000	;8 bit DAC.
   JCR ← 120000	;Joint control output register.
   ENCL ← 140000	;Encoder mux low.
   ENCH ← 140001	;Encoder mux high.

   STKSIZ ← 377	;Stack size.
   OFF ← 377	;?
   LSBENB ← 40	;Enable LSB servo.
   DBLMOD ← 20	;Double all position commands for
	;joints that have extremely low gearing.

;Zero page variables.
;Not shared?

IOCTRL:	0	;Copy of JCR output port.
CURVEL:	BLOCK 2	;Current velocity.
	0
SETPT:	BLOCK 3	;Current setpoint.
	0
SETINC:	BLOCK 3	;Interpolating increment for setpoints.
PREDCT:	BLOCK 3	;Result of the predictive term.
LSTINX:	BLOCK 3	;Position at last index pulse.
OLDSP:	BLOCK 3	;Last commanded setpoint, for CMDVEL.
POSERR:	BLOCK 3	;Current position error.
DACSIG:	BLOCK 3	;Scratch.

BGLOCK:	0	;Interlock around background pre. cal.
DSPAT:	BLOCK 2	;Dispatch address when cmds are rcvd?
DSPAT2:	BLOCK 2	;Dispatch when commands are executed?
INCTR:	0	;Count the interpolations.
HSTTMR:	0	;Count ticks between host commands.

LOGTMP:	BLOCK 4	;Temp for the arithmetic routines.
CVSAV:	BLOCK 2	;Save area for background variables.
VELSAV:	BLOCK 2
BGTMP:	BLOCK 2

ZAPEND ← .-1	;Clear all the above in startup.

CURPOS:	BLOCK 3	;Current position, extended to 3 bytes.

TL:	0	;Scratch for grey to binary.
TH:	0
;Shared ram. Check all refs.
   LOC 200	;Second half of zero page.

STATUS:	0	;Flags for the host.
MODE:	0	;Mode bits from host.

CKWORD:	BLOCK 2	;Host I/O check/command word.
CMDPOS:	BLOCK 2	;Commanded position from host.

BUSLO:	0	;Buffer?
BUSHI:	0	;?

MEMPTR:	BLOCK 2	;Address pointer for diagnostic read.
;NINTER = function of INTSCL?
NINTER:	0	;# of interpolations between position
		;commands.
INTSCL:	0	;# of bits to shift setpoint dif for
		;interpolating.
HSTLIM:	0	;# of clock ticks allowed between host
	0	;commands.
CMDVEL:	BLOCK 2	;Commanded velocity. Not shared?
MASS:	BLOCK 2	;Inertia term for prediction.
FRICTN:	BLOCK 2	;Viscous damping coefficient.
GRAVTY:	BLOCK 4	;DC offset for gravity.
POSTOL:	BLOCK 4	;Half-width of position tolerance band.
INTTOL:	BLOCK 4	;Half-width of integration band.


;Start of prom.
   LOC 174000

INITBL:	STATUS	↔	200
	MODE	↔	0	;?

	NINTER	↔	=32
	INTSCL	↔	5

	HSTLIM	↔	=48
	HSTLIM+1↔	0	;?

	DSPAT+1	↔	(IMBLK⊗-10)∧377
	DSPAT2+1   ↔	(DFBLK⊗-10)∧377

	377
;Add SEI and CLI to all shared ram refs.
;Add ? for IOCTRL read?
START:	CLD
	LDXI	STKSIZ	;Setup stack.
	TXS

;Zero shared ram?
	LDAI	0
	LDXI	ZAPEND
RLOOP:	STAZX	0	;Reset ram.
	DEX
	BPL	RLOOP
	STAZ	CURPOS+2;?
	STA	DAC	;Clear DAC.

	TAY
	BEQ	RSTDEF	;Jump


DLOOP:	INY
	LDAY	INITBL	;Init ram.
	STAZX	0
	INY

RSTDEF:	LDXZY	INITBL
	CPXI	377
	BNE	DLOOP

	JSR	POSUPD	;?
	JSR	SETPOS	;?

	CLI

RSTCKW:	LDAI	0	;Reset check word.
	SEI
	STAZ	CKWORD
	STAZ	CKWORD+1
	CLI
;Idle loop. Wait for command.
IDLE:	LDXZ	CKWORD+1;+1 for no lock.
	BEQ	IDLE

	SEI
	LDAZ	CKWORD	;Get check word.
	LDXZ	CKWORD+1
	CLI

;CKWORD is command?
	SEI
	LDAZ	CMDPOS	;Get position.
	LDXZ	CMDPOS+1
	CLI
;Position command?
CMDSRV:	JSR	ENBTST	;?
	JSR	DOUBLE
	STAZ	DACSIG
	STXZ	DACSIG+1
	STYZ	DACSIG+2

	SEC
	SBCZ	SETPT
	STAZ	SETINC
	TXA
	SBCZ	SETPT+1
	STAZ	SETINC+1
	TYA
	SBCZ	SETPT+2
	LDXI	0
	STXZ	SETPT-1
	STXZ	SETINC-1
	LDXZ	INTSCL

SCAL:	CMPI	200	;Extend sign.
	RORA
	RORZ	SETINC+1
	RORZ	SETINC
	RORZ	SETINC-1
	DEX
	BNE	SCAL

	STAZ	SETINC+2
	LDAZ	NINTER
	STAZ	INCTR
	SEC
	LDAZ	DACSIG
	SBCZ	OLDSP
	STAZ	CMDVEL
	LDAZ	DACSIG+1
	SBCZ	OLDSP+1
	STAZ	CMDVEL+1
	LDAZ	DACSIG
	STAZ	OLDSP
	LDAZ	DACSIG+1
	STAZ	OLDSP+1
	LDAZ	DACSIG+2
	STAZ	OLDSP+2

	LDAZ	IOCTRL
	ORAI	44
	STAZ	IOCTRL
	STA	JCR	;Output it.

	LDAZ	HSTLIM	;Reset host timer.
	STAZ	HSTTMR
	JMP	CMDEND
;Clock tick interrupt.
TICK:	PHA	;Save state.
	TXA
	PHA
	TYA
	PHA

	LDY	ENCL	;Read encoder.
	LDA	ENCH

;Convert from grey to binary.
	STAZ	TH
	LSRA
	EORZ	TH
	STAZ	TH
	TAX

	TYA
	STAZ	TL
	RORA
	EORZ	TL
	STAZ	TL

	LSRZ	TH
	RORA
	LSRZ	TH
	RORA

	EORZ	TL
	STAZ	TL
	TAY
	TXA
	EORZ	TH
	STAZ	TH

	LSRA
	RORZ	TL
	LSRA
	RORZ	TL
	LSRA
	RORZ	TL
	LSRA
	RORZ	TL

	EORZ	TH
	STAZ	TH
	TYA
	EORZ	TL
	EORZ	TH
	STAZ	TL	;?

;Extend sign from n bits?
;	LDXZ	TH	 ?

	TAY		;?
	LDAZ	TH	;?
	JSR	POSUPD	;Put POSUPD here?

	STAZ	CURPOS
	STXZ	CURPOS+1
	STYZ	CURPOS+2

	DECZ	HSTTMR
	BPL	HOSTOK

	LDAI	0
	STAZ	HSTTMR
	STAZ	CMDVEL
	STAZ	CMDVEL+1

;IOCTRL is copy of JCR?
HOSTOK:	LDAI	4	;?
	BITZ	IOCTRL	;?If position mode is off,
	BNE	INTRS
	JMP	CURSRV	;don't servo.

;Interpolate the setpoints.
INTRS:	CLC
	LDAZ	SETPT-1
	ADCZ	SETINC-1
	STAZ	SETPT-1
	LDAZ	SETPT
	ADCZ	SETINC
	STAZ	SETPT
	LDAZ	SETPT+1
	ADCZ	SETINC+1
	STAZ	SETPT+1
	LDAZ	SETPT+2
	ADCZ	SETINC+2
	STAZ	SETPT+2

	DECZ	INCTR
	BNE	GPOSER

	LDAI	0
	STAZ	SETINC-1
	STAZ	SETINC
	STAZ	SETINC+1
	STAZ	SETINC+2

;Calculate the position error.
GPOSER:	SEC
	LDAZ	CURPOS
	SBCZ	SETPT
	STAZ	POSERR
	LDAZ	CURPOS+1
	SBCZ	SETPT+1
	STAZ	POSERR+1
	LDAZ	CURPOS+2
	SBCZ	SETPT+2
	STAZ	POSERR+2
	BITZ	MODE	;?If servo is disabled, we're
	BPL	OOTOL	;automatically out of tolerance

	LDAZ	POSERR+2;Test the sign of pos error.
	BMI	NEGPER

	LDAZ	POSTOL	;Positive. Compare with tol.
	CMPZ	POSERR
	LDAZ	POSTOL+1
	SBCZ	POSERR+1
	LDAI	0
	SBCZ	POSERR+2
	BCS	TOLOK	;In tolerance.
	BCC	OOTOL	;Jump.

NEGPER:	CLC	;Negative. Add the tolerance.
	LDAZ	POSTOL
	ADCZ	POSERR
	LDAZ	POSTOL+1
	ADCZ	POSERR+1
	LDAI	0
	ADCZ	POSERR+2
	BCS	TOLOK	;In tolerance.

OOTOL:	LDAZ	IOCTRL	;Out of tolerance.
	ANDI	177	;Turn off the in tolerance
	BNE	WCNTRL	;indicator.

TOLOK:	LDAZ	IOCTRL	;In tolerance. Turn it on.
	ORAI	200
WCNTRL:	STAZ	IOCTRL
	STA	JCR	;Copy it to output.

	BITZ	MODE	;If intergration is disabled,
	BVC	OOBAND	;turn it off.
	LDAZ	POSERR+2;Test sign of position error.
	BMI	ADTOL

	LDAZ	INTTOL	;Positive. Compare with tol.
	CMPZ	POSERR
	LDAZ	INTTOL+1
	SBCZ	POSERR+1
	LDAI	0
	SBCZ	POSERR+2
	BCS	INBAND
	BCC	OOBAND

ADTOL:	CLC	;Negative. Add the tolerance.
	LDAZ	INTTOL
	ADCZ	POSERR
	LDAZ	INTTOL+1
	ADCZ	POSERR+1
	LDAI	0
	ADCZ	POSERR+2
	BCS	INBAND

OOBAND:	LDAZ	IOCTRL	;Out of band. Turn off
	ORAI	10	;integration by setting the
	ANDI	357	;control bit. LSB servo off.
	BNE	WCTRL2
INBAND:	LDAI	LSBENB	;In band. Is LSB servo enabled
	BITZ	MODE
	BEQ	RCNTRL

	LDAZ	POSERR	;Yes. Is the error exactly 0?
	ORAZ	POSERR+1
	ORAZ	POSERR+2
	BNE	RCNTRL

	LDAZ	IOCTRL	;It is. Integration off, LSB
	ORAI	30	;servo on.
	BNE	WCTRL2	;Jump.

RCNTRL:	LDAZ	IOCTRL	;LSB disabled or error
	ANDI	347	;not zero. LSB servo off,
			;integration on.

WCTRL2:	STAZ	IOCTRL
	STA	JCR	;Output it.
	LDAZ	LOGTMP	;Since the arithmetic routines
	LDYZ	LOGTMP+1;aren't re-entrant, we need to
	STAZ	LOGTMP+2;save their state here.
	STYZ	LOGTMP+3

	LDYZ	CURVEL	;Get the velocity,
	LDAZ	CURVEL+1
	JSR	LOG
	LDXI	FRICTN	;mult. by the friction
		;FRICTN+1 for no memory lock?
	JSR	MULTIP	;coefficient,
	JSR	EXP
	TAX
	TYA
	CLC	;add the position error...
	ADCZ	POSERR
	STAZ	DACSIG
	TXA
	ADCZ	POSERR+1
	STAZ	DACSIG+1
	LDYI	0
	TXA	;(sign-extend the velocity)
	BPL	NODEY
	DEY

NODEY:	TYA
	ADCZ	POSERR+2
	STAZ	DACSIG+2

	CLC	;...the velocity predictive term...
	LDAZ	DACSIG
	ADCZ	PREDCT
	STAZ	DACSIG
	LDAZ	DACSIG+1
	ADCZ	PREDCT+1
	STAZ	DACSIG+1
	LDAZ	DACSIG+2
	ADCZ	PREDCT+2
	STAZ	DACSIG+2

	CLC	;...and the gracity offset.
	LDAZ	DACSIG
	SEI
	ADCZ	GRAVTY
	TAY
	LDAZ	DACSIG+1
	ADCZ	GRAVTY+1
	CLI
	TAX
	LDAZ	DACSIG+2
	ADCZ	GRAVTY+2;GRAVTY+3?

	JSR	PUTDAC	;Put result out to the DAC.
;Put PUTDAC here? Not a subroutine?

	LDYZ	LOGTMP+3;Restore the arithmetic
	LDAZ	LOGTMP+2;routines' state.
	STYZ	LOGTMP+1
	STAZ	LOGTMP

CMDSP:		;Add deferred commands here?
;Change CMDEND for no host command interrupt?
CMDEND:	LDAI	4	;Done with commands.
	BITZ	IOCTRL	;Are we servoing?
	BEQ	INTXIT
	BITZ	BGLOCK	;Yes. Is the background
	BMI	INTXIT	;predictor still running?

	DECZ	BGLOCK	;No. Start it up.
	JMP	BGSRV

BGDON:	INCZ	BGLOCK	;Unlock?

INTXIT:	PLA	;Restore state and dismiss interrupt.
	TAY
	PLA
	TAX
	PLA
	RTI

CURSRV:		;Not servoing ("Current mode")...
   ;Add stop mode?
	JMP	CMDSP

;Background velocity prediction.
BGSRV:	LDAZ	CURVEL	;Copy the variables used to
	STAZ	VELSAV	;avoid interference from
	LDAZ	CURVEL+1;interrupts while this routine
	STAZ	VELSAV+1;is running.
	LDAZ	CMDVEL
	STAZ	CVSAV
	LDAZ	CMDVEL+1
	STAZ	CVSAV+1
	LDYZ	POSERR
	LDAZ	POSERR+1
	LDXZ	POSERR+2

	CLI	;Enable interrupts?

	PHA
	ASLA	;Is magnitude of position error
	TXA	;< 2↑15?
	ADCI	0
	BEQ	FLOERR

	PLA	;No. Set the predictive term to zero.
	LDAI	0
	TAX
	TAY
	JMP	NTRLOC

FLOERR:	PLA	;Yes. Float the position error.
	JSR	LOG
	JSR	INV	;TMP = 1 / POSERR
	STYZ	BGTMP
	STAZ	BGTMP+1
	CLC
	LDAZ	CVSAV	;Commanded velocity + current
	ADCZ	VELSAV	;velocity...
	TAY
	LDAZ	CVSAV+1
	ADCZ	VELSAV+1
	JSR	LOG	;...float...
	LDXI	BGTMP
	JSR	MULTIP	;...* TMP...
	STYZ	BGTMP	;...stored at TMP.
	STAZ	BGTMP+1
	SEC
	LDAZ	CVSAV	;Commanded velocity - current
	SBCZ	VELSAV	;velocity...
	TAY
	LDAZ	CVSAV+1
	SBCZ	VELSAV+1
	JSR	LOG	;...same thing.
	LDXI	BGTMP
	JSR	MULTIP
	STYZ	BGTMP
	STAZ	BGTMP+1

	SEI	;Interlock...

	LDYZ	MASS	;...get the mass...
	LDAZ	MASS+1

	CLI	;clear the lock.

	JSR	MULTIP	;Scale the predictor.
	JSR	EXP	;Back to integer form.
	LDXI	0
	CMPI	0
	BPL	NTRLOC	;Extend sign to 3 bytes.
	DEX

NTRLOC:	SEI	;End of background. Interlock.

	STYZ	PREDCT
	STAZ	PREDCT+1;Store the result for the servo
	STXZ	PREDCT+2;to use.
	JMP	BGDON
INTBL:		;IMMEDIATE COMMAND TABLE?
	HCIRDM∧377	;Read memory.

	HCISRV∧377	;Position mode?

CMTBL:		;DEFERRED COMMAND TABLE?
	CMDEND∧377	;Read memory?

	CMDSRV∧377	;Position mode?
;Subroutines?
;Enter with position in A (low), X (middle), Y (high).
;Sets current position to that value, puts the setpoint
;to the same, clears the setpoint interpolating
;increment, and goes into stop mode.
;??
SETPOS:	STAZ	CURPOS	;Set the current position.
	STXZ	CURPOS+1
	STYZ	CURPOS+2

;Second entry - freeze to the position in A, X, Y as
;above without changing the current position.
;??
FREZE:	STAZ	SETPT	;Set the position command.
	STXZ	SETPT+1
	STYZ	SETPT+2
	STAZ	OLDSP
	STXZ	OLDSP+1
	STYZ	OLDSP+2

	LDAI	75	;I/O control bits for servo
	STAZ	IOCTRL	;enable on, all others off.
	STA	JCR

	LDAI	0
	STAZ	SETPT-1	;Clear the setpoint extension
	STAZ	SETINC-1;and the interpolator
	STAZ	SETINC
	STAZ	SETINC+1
	STAZ	SETINC+2
	STAZ	CMDVEL	;and the commanded velocity.
	STAZ	CMDVEL+1

	LDAZ	SETPT	;Return the regs. unchanged.
	RTS

;Enter with low counter value in Y.
;Returns updated position in A (low), X (middle),
;Y (high). Also sets CURVEL to the 16-bit signed
;velocity.
;??
POSUPD:	STAZ	DACSIG+1;Save high byte.
	TYA
	STAZ	DACSIG	;Save that value.
	SEC
	SBCZ	CURPOS	;Subtract the old position
	STAZ	CURVEL	;yielding the velocity.
	LDAZ	DACSIG+1
	SBCZ	CURPOS+1
	STAZ	CURVEL+1
	LDXZ	CURPOS+1	;Set up for updating bytes
	LDYZ	CURPOS+2	;2 and 3.
	LDAZ	DACSIG+1;Did bit 15 of pos. change?
	EORZ	CURPOS+1
	BPL	GETDAC	;If not, we're through.
	LDAZ	CURVEL+1;It did. Which way did we move
	BMI	DOWN
	LDAZ	DACSIG+1;Upward.
	BMI	GETDAC	;If bit 15 is on, we're done.
	INY	;Off. Increment high byte.
	JMP	GETDAC

DOWN:	LDAZ	DACSIG+1;Downward.
	BPL	GETDAC	;If bit 15 is off, we're done.
	DEY	;Decrement high byte.

GETDAC:	LDAZ	DACSIG
	RTS
;DAC output subroutine. Not sub?
;Enter with 3 byte value in Y (low), X (middle),
;A (high). Clobbers all registers, but the 8 bits the
;DAC got are returned in?
PUTDAC:	BMI	NEGDAC	;Assuming the last I. loaded A.
	CPYI	200	;Positive. Compare with 2↑7.
	BCS	TOOHI
	CPXI	1
	SBCI	0
	BCC	INRNGE

TOOHI:	LDYI	177	;Too high. Saturate positive.
	BNE	INRNGE	;Jump.

NEGDAC:	CPYI	200	;Negative. Compare with -2↑7.
	BCC	TOOLOW

	CPXI	377
	SBCI	377
	BCS	INRNGE

TOOLOW:	LDYI	200	;Too low. Saturate to -2↑7.

INRNGE:	STY	DAC	;Output 8 bits to the DAC.
	RTS

   ;Delete DOUBLE and HALVE?
DOUBLE:	PHA	;Doubles the position in (Y,X,A) if
	LDAI	DBLMOD	;the double mode bit is set.
	BITZ	MODE
	BEQ	NOTDBL
	PLA
	ASLA
	PHA
	TXA
	ROLA
	TAX
	TYA
	ROLA
	TAY
NOTDBL:	PLA
	RTS

HALVE:	PHA	;Halve the position argument in (Y,X,A)
	LDAI	DBLMOD	;if the double mode bit is set.
	BITZ	MODE
	BEQ	NOTDBL
	TYA
	CMPI	200
	RORA
	TAY
	TXA
	RORA
	TAX
	PLA
	RORA
	RTS
;No index?
ENBTST:	PHA	;Test for servo enabled?
	LDAZ	MODE
	ANDI	202	;?
	CMPI	200
	BNE	NOTENB
	PLA	;OK. Return.
	RTS

NOTENB:	PLA	;No. Wipe the return address and
	PLA	;end this command.
	PLA
	JMP	CMDEND
;Arithmetic routines.
;Enter with high byte in A, low in Y.
;Returns A = characteristic and sign, Y = mantissa.
;Clobbers X, LOGTMP, LOGTMP+1.
LOG:	STYZ	LOGTMP	;Save the inputs.
	STAZ	LOGTMP+1

	LDXI	20+100	;?Init characteristic to 15.
	CMPI	0	;Test sign of input.
	BPL	POSIN
	SEC	;Negative. 2's complement it.
	LDAI	0
	SBCZ	LOGTMP
	STAZ	LOGTMP
	LDAI	0
	SBCZ	LOGTMP+1
POSIN:	BNE	NORML	;Is high byte zero?
	LDAZ	LOGTMP	;Yes. Low byte?
	BEQ	RTRN	;If so, return zero.
	LDYI	0	;Low nonzero. Shift left one
	STYZ	LOGTMP	;byte,
	LDXI	10+100	;?change characteristic to 7.
NORML:	DEX	;Normalize the number, counting the
	ASLZ	LOGTMP	;characteristic down. When the
	ROLA	;first "1" shifts out, we've subtracted
	BCC	NORML	;1 from the normalized number
	ASLZ	LOGTMP	;(This rounds the result)
	ADCI	=11	;and are left with the fraction
	TAY	;Adding 11 to that is equivalent to
	TXA	;adding 0.043.
	ADCI	0	;Propagate the carry into the
			;characteristic.
	ASLA	;Insert the sign bit from the saved
	ASLZ	LOGTMP+1;input.
	RORA
RTRN:	RTS	;Done.

;Enter with sign and characteristic in A, mantissa in Y
;Returns 16-bit integer, low byte in Y, high in A.
;Clobbers X, LOGTMP, LOGTMP+1.
EXP:	STAZ	LOGTMP+1;Save sign of input.
	ANDI	177	;Mask it off.
	BEQ	ZEROIN	;Zero characteristic returns
	TAX	;zero.
	TYA	;Get the mantissa...
	SEC
	SBCI	=11	;...subtract 0.043...
	STAZ	LOGTMP	;(save this value)
	TXA	;...propagate the carry and get rid
	SBCI	100	;of the XS-64 offset.
	BMI	NEGIN	;If negative (value < 1.0)
			;return zero.
	CMPI	=15	;Test for overflow (value>=2↑15
	BCS	SATUR
	TAX	;...no. Number is in range.
	ADCI	-10	;?Is characteristic below 8?
	BMI	BLOATE
	TAX	;No. Reduce if by 8,
	JSR	UNNORM	;unnormalize.
	BMI	GETTMP	;Jump.
BLOATE:	JSR	UNNORM	;Yes. Unnormalize, then
	ASLZ	LOGTMP	;(round result)
	ADCI	0
	STAZ	LOGTMP	;use result as low byte and
	LDAI	0	;set high byte to zero.

GETTMP:	LDYZ	LOGTMP
GTMP1:	LDXZ	LOGTMP+1;Test sign of input...
	BPL	POSIGN
	STAZ	LOGTMP+1;...negative. 2's complement
	LDAI	0	;the result.
	SEC
	SBCZ	LOGTMP
	TAY
	LDAI	0
	SBCZ	LOGTMP+1
POSIGN:	RTS

NEGIN:	LDAI	0	;Set the result to zero if the
ZEROIN:	TAY	;input is negative.
	RTS

SATUR:	LDYI	OFF	;Saturate result to 2↑15 - 1 if
	STYZ	LOGTMP	;input was 15 or more.
	LDAI	177
	BNE	GTMP1	;Jump.

UNNORM:	LDAI	1	;Unnormalize subroutine. Add 1
	BNE	DECRX	;to the fraction.

SCALE:	ASLZ	LOGTMP	;Scale the fraction left by the
	ROLA	;amount of the characteristic.
DECRX:	DEX
	BPL	SCALE
	RTS

;Enter with characteristic of multiplier in A,
;mantissa in Y, X pointing to a pair of base page
;locations containing the multiplicand (mantissa in the
;low byte).
;Returns the product in A and Y, same form as the
;multiplier. Leaves X unchanged. Clobbers LOGTMP and
;LOGTMP+1.
MULTIP:	PHA
	EORZX	1	;Compute sign of result,
	STAZ	LOGTMP+1	;save it away.
	PLA
	ANDI	177	;Mask off multiplier sign.
	BEQ	ZEROIN	;If zero, return zero.
	STAZ	LOGTMP
	TYA	;Add the two logarithms.
	CLC
	ADCZX	0
	TAY
	LDAZX	1
	ANDI	177	;If multiplicand is zero,
	BEQ	ZEROIN	;return a zero.
	ADCZ	LOGTMP
	SEC
	SBCI	100	;Correct the XS-64 offset.
	BPL	INSIGN	;Result in range?
	ANDI	100	;No. If underflow,
	BNE	NEGIN	;return zero.
	LDAI	177	;Overflow. Saturate to
	LDYI	377	;highest magnitude.

INSIGN:	ASLA	;Insert the sign of the result.
	ASLZ	LOGTMP+1
	RORA
	RTS

;Inverse function: 2's complement the magnitude part
;of a 15-bit logarithm.
;Enter with characteristic in A, mantissa in Y.
;Returns inverse in the same form. X unchanged.
;Clobbers LOGTMP and LOGTMP+1.
INV:	STYZ	LOGTMP	;Pretty straightforward...
	STAZ	LOGTMP+1
	SEC
	LDAI	0	;Complement the number by
	SBCZ	LOGTMP	;subtracting it from zero.
	TAY
	LDAI	0
	SBCZ	LOGTMP+1
	JMP	INSIGN	;Insert the original sign.
;DEFERRED COMMANDS.
;Fix GRAVTY+2, POSTOL+2, and INTTOL+2.
;Add DOUBLE to POSTOL and INTTOL.

   LOC (.∨377)+1	;For start of next page.
DFBLK ← .

;Set parameter command?
CMDSET:	STAZX	0	;?
	JMP	CMDEND

;CMDCUR:	Stop mode?
;Immediate commands.

   LOC (.∨377)+1	;For start of next page.
IMBLK ← .
HCISRV:		;?

;Sync, ack?
HCIRDM:	LDYZ	MEMPTR
	LDAY	0
	LDXY	1
	INY
	INY
	STYZ	MEMPTR

	STAZ	BUSLO	;?
	STXZ	BUSHI
	JMP	INTXIT

;Add HALVE to CURPOS. Fix CURPOS+2.

HCINOP:		;?
   ;Ack host.
	JMP	INTXIT

   NMI ← START	;Reset??
;Interrupt vectors.
   LOC 177772
	NMI∧377
	(NMI⊗-10)∧377
	START∧377
	(START⊗-10)∧377
	TICK∧377
	(TICK⊗-10)∧377
END